/*
    This source file is part of GeNa project.
    Copyright © GeNa project 2008
    For the latest info, see http://code.google.com/p/genaproject/ or  http://lumpyproject.forums-rpg.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Foobar is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar.  If not, see <http://www.gnu.org/licenses/>. */

#ifndef QUATERNION_H_INCLUDED
#define QUATERNION_H_INCLUDED

#include <boost/operators.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
using namespace boost;

#include <cmath>
#include <cfloat>

namespace math
{

template<class T> class Vector3;
template<class T> class Vector4;
template<class T> class Matrix3;
template<class T> class Matrix4;

template<class T> class Quaternion :    equality_comparable< Quaternion<T>,
                                        addable< Quaternion<T>,
                                        subtractable< Quaternion<T>,
                                        dividable< Quaternion<T>,
                                        multipliable< Quaternion<T>, T,
                                        dividable< Quaternion<T>, T
                                        > > > > > >
{
public:
    T* l;
    struct
    {
        T x,y,z,w;
    };

    static const Quaternion Identity;
    static const Quaternion Null;

    Quaternion();

    Quaternion( T _w, T _x, T _y, T _z);

    template<class U> Quaternion( const Quaternion<U> &q);


    Quaternion& operator=( const Quaternion &q);

    template<class U> Quaternion( Vector3<U> v, angle phi); //Angle is between -1 and 1

    //Operators
    bool operator==( const Quaternion &q) const; //You should not use this test with floating point component !!!

    Quaternion & operator+=(Quaternion &q);

    Quaternion & operator-=(const Quaternion &q);

    Quaternion & operator*=(T k);

    Quaternion operator*(const Quaternion &q) const;

    Quaternion & operator*=(const Quaternion &q);

    Quaternion conjugate();

    Quaternion inverse();

    T squaredNorm();

    T norm();

    Quaternion unitQuaternion();

    Quaternion & operator/=(T k);

    Quaternion operator/(const Quaternion &q);

    Matrix3<T> toMatrix3();

    Matrix4<float> toMatrix4();

    template<class U> Vector3<U> transform(const Vector3<U> &v);

    template<class Archive> void serialize(Archive & ar, const unsigned int version);

};

#include "matrix.h"
#include "vector.h"

template<class T> Quaternion<T>::Quaternion() : l(&x)
{
    w = 1.0;
    x = 0.0;
    y = 0.0;
    z = 0.0;
}

template<class T> Quaternion<T>::Quaternion( T _w, T _x, T _y, T _z) : l(&x)
{
    w = _w;
    x = _x;
    y = _y;
    z = _z;
}


template<class T> template<class U> Quaternion<T>::Quaternion( const Quaternion<U> &q) : l(&x)
{
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;
}

template<class T> Quaternion<T>& Quaternion<T>::operator=( const Quaternion &q)
{
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;
    return *this;
}

template<class T> template<class U> Quaternion<T>::Quaternion( Vector3<U> v, angle phi)
{
    angle phiD2 = phi/angle(2.0f);

    w = cos(phiD2);

    vector3a v2 = v.getNormalized();
    v2 *= sin(phiD2);

    x = v2.x;
    y = v2.y;
    z = v2.z;
}

//Operators
template<class T> bool Quaternion<T>::operator==( const Quaternion &q) const //You should not use this test with floating point component !!!
{
    return (x == q.x) && (y == q.y) && (z == q.z) && (w == q.w);
}

template<class T> Quaternion<T> & Quaternion<T>::operator+=(Quaternion &q)
{
    x += q.x;
    y += q.y;
    z += q.z;
    w += q.w;
    return *this;
}

template<class T> Quaternion<T> & Quaternion<T>::operator-=(const Quaternion &q)
{
    x -= q.x;
    y -= q.y;
    z -= q.z;
    w -= q.w;
    return *this;
}

template<class T> Quaternion<T> & Quaternion<T>::operator*=(T k)
{
    x *= k;
    y *= k;
    z *= k;
    w *= k;
    return *this;
}

template<class T> Quaternion<T> Quaternion<T>::operator*(const Quaternion &q) const
{

    Quaternion r;
    r.w = w*q.w - x*q.x - y*q.y - z*q.z;
    r.x = w*q.x + x*q.w + y*q.z - z*q.y;
    r.y = w*q.y - x*q.z + y*q.w + z*q.x;
    r.z = w*q.z + x*q.y - y*q.x + z*q.w;

    return r;
}

template<class T> Quaternion<T> & Quaternion<T>::operator*=(const Quaternion &q)
{

    *this = (*this)*q;
    return *this;
}

template<class T> Quaternion<T> Quaternion<T>::conjugate()
{
    return Quaternion(w, -x, -y, -z);
}

template<class T> Quaternion<T> Quaternion<T>::inverse()
{
    T s = squaredNorm();
    if( s < (T)FLT_EPSILON)
        return Quaternion();
    return conjugate()/s;
}

template<class T> T Quaternion<T>::squaredNorm()
{
    return w*w + x*x + y*y + z*z;
}

template<class T> T Quaternion<T>::norm()
{
    return ::sqrt( w*w + x*x + y*y + z*z);
}

template<class T> Quaternion<T> Quaternion<T>::unitQuaternion()
{
    return (*this)/norm();
}

template<class T> Quaternion<T> & Quaternion<T>::operator/=(T k)
{
    x /= k;
    y /= k;
    z /= k;
    w /= k;
    return *this;
}

template<class T> Quaternion<T> Quaternion<T>::operator/(const Quaternion &q)
{
    return (*this)*q.inverse();
}

template<class T> Matrix3<T> Quaternion<T>::toMatrix3()
{
    T Nq = squaredNorm();

    T s = Nq > 0.0 ? 2.0/Nq : 0.0;

    T xs = x*s, ys = y*s, zs = z*s;
    T wxs = w*xs, wys = w*ys, wzs = w*zs;
    T xxs = x*xs, xys = x*ys, xzs = x*zs;
    T yys = y*ys, yzs = y*zs, zzs = z*zs;

    return Matrix3<T>( 1 - (yys + zzs),    xys - wzs,          xzs + wys,
                    xys + wzs,          1 - (xxs + zzs),    yzs - wxs,
                    xzs - wys,          yzs + wxs,          1 - (xxs + yys));
}


template<class T> Matrix4<float> Quaternion<T>::toMatrix4()
{
    float _x = x;
    float _y = y;
    float _z = z;
    float _w = w;

    float Nq = _x*_x + _y*_y + _z*_z + _w*_w;

    float s = Nq > 0.0 ? 2.0/Nq : 0.0;

    float xs = _x*s, ys = _y*s, zs = _z*s;
    float wxs = _w*xs, wys = _w*ys, wzs = _w*zs;
    float xxs = _x*xs, xys = _x*ys, xzs = _x*zs;
    float yys = _y*ys, yzs = _y*zs, zzs = _z*zs;

    return Matrix4<float>(1 - (yys + zzs),    xys - wzs,          xzs + wys,       0,
                       xys + wzs,          1 - (xxs + zzs),    yzs - wxs,       0,
                       xzs - wys,          yzs + wxs,         1 - (xxs + yys), 0,
                       0,                  0,                  0,               1);
}

template<class T> template<class U> Vector3<U> Quaternion<T>::transform(const Vector3<U> &v)
{
    Quaternion<U> q( 0.0, v.x, v.y, v.z);
    q = (Quaternion<U>)(*this) * q * (Quaternion<U>)(this->conjugate());
    return Vector3<U>(q.x, q.y, q.z);
}


template<class T> template<class Archive> void Quaternion<T>::serialize(Archive & ar, const unsigned int version)
{
    ar & boost::serialization::make_nvp("l",serialization::make_array(l,4));
}


template< class T> const Quaternion<T> Quaternion<T>::Identity(1.0,0.0,0.0,0.0);
template< class T> const Quaternion<T> Quaternion<T>::Null(0.0,0.0,0.0,0.0);

} //namespace math

#include "math/fixed.h"

namespace math
{
typedef Quaternion<angle> quaternionq;
typedef Quaternion<float> quaternionf;
} //namespace math



#endif // QUATERNION_H_INCLUDED
